import matplotlib.pyplot as plt
import numpy as np
import math
#定义初始值-------------------------------------------------------------------------------------

g = 32.2
a0 = 400000
a1 = -6000
a2 = -0.5*g
delta_T = 0.1                             #采样时间间隔

T = 30                                    #总时间
t_0 = np.arange(0, T, 0.1)               #采样点
Xk = [0]*len(t_0)

for i in range(0,len(t_0)):
    Xk[i] = a0+a1*t_0[i]+a2*(t_0[i]**2)   #真实值
K = list(range(1,int(T/delta_T)+1))       #采样次数

X_measure = list(range(0,int(T/delta_T))) #测量值
t = [0]*len(K)
xhat_pre = np.array([[0],[0]])        #估计值的初始值 2 * 1
xhat = xhat_pre                           #估计值的集合
e0 = [0]*len(K)
e1 = [0]*len(K)
#e2 = [0]*len(K)

Pk = np.array([[99999999,0],
               [0,99999999]])
φk = np.array([[1,delta_T],
               [0,1]])
H = np.array([[1,0]])#注意看有几个[]
Rk = 50000.0
PHIS = 0   #条件  加了过程噪声
Q = np.array([[PHIS*(delta_T**3)/3,PHIS*(delta_T**2)/2],
              [PHIS*(delta_T**2)/2,PHIS*(delta_T)]]);

SP11 = [0]*len(K)                         #用来画限制线
SP11D = [0]*len(K)
SP22 = [0]*len(K)
SP22D = [0]*len(K)
#SP33 = [0]*len(K)
#SP33D = [0]*len(K)
#得出具体的采样时间数组-----------------------------------------------------------------------------


for i in range(0,len(K)):
    t[i] = (K[i]-1)*delta_T

#计算测量值（加上测量噪声）----------------------------------------------------------------------
for i in range(0,len(K)):
    vk = np.random.normal(loc=0.0, scale=100.0, size=None)
    x = np.array([[a0 + a1 * t[i] + a2 * (t[i] ** 2)], [a1 + 2 * a2 * t[i]]])
    X_measure[i] = float(H.dot(x)) + vk
# 计算估计值---------------------------------------------------------------------------------------
for i in range(0, len(K)):
    φk_T = np.transpose(φk)
    H_T = np.transpose(H)
    Mk = φk.dot(Pk).dot(φk_T) + Q
    CC = np.linalg.inv(H.dot(Mk).dot(H_T) + Rk)
    Kk = Mk.dot(H_T).dot(CC)
    o = φk.dot(xhat_pre) + Kk.dot(X_measure[i] - H.dot(φk).dot(xhat_pre))
    Pk = (np.eye(2) - Kk.dot(H)).dot(Mk)  # 更新误差协方差矩阵
    # np.eye(3)返回的是一个二维2的数组(N,M)，对角线的地方为1，其余的地方为0.
    SP11[i] = math.sqrt(Pk[0, 0])
    SP11D[i] = -SP11[i]
    SP22[i] = math.sqrt(Pk[1, 1])
    SP22D[i] = -SP22[i]
    #SP33[i] = math.sqrt(Pk[2, 2])
    #SP33D[i] = -SP33[i]
    xhat_pre = o
    xhat = np.c_[xhat, o]
xhat = np.delete(xhat, 0, axis=1)
# 计算误差值---------------------------------------------------------------------------------------
for i in range(0, len(K)):
    e0[i] = (a0 + a1 * t[i] + a2 * (t[i] ** 2)) - xhat[0, i]
    # e0[i] = xhat[0, i] - (a0 + a1 * t[i] + a2 * (t[i] ** 2))  # 估计值和真实值的误差
    e1[i] = (a1 + 2 * a2 * t[i]) - xhat[1, i]
    # e1[i] = xhat[1, i] - (a1 + 2 * a2 * t[i])  # 估计值的导数和真实值的误差
    #e2[i] = xhat[2, i] - (2 * a2)  # 估计值的二阶导数和真实值的误差
# 画图---------------------------------------------------------------------------------------------
plt.figure()
plt.xlabel("Time(Sec)")
plt.ylabel("xhat")
plt.plot(t, X_measure, '-o', c='k', label='measures')
plt.plot(t, xhat[0], c="r", label='estimates')
plt.plot(t_0, Xk, c="b", label='true')
plt.legend()

plt.figure()
plt.xlabel("Time(Sec)")
plt.ylabel("d-xhat")
plt.ylim(-7000, -5900)
plt.plot(t, xhat[1], c="k", label='estimates')
plt.plot(t_0, a1 + 2 * a2 * t_0, '--', c='r', label='true')
plt.legend()

#plt.figure()
#plt.xlabel("Time(Sec)")
#plt.ylabel("d2-xhat")
#plt.ylim(-40, -5)
#plt.plot(t, xhat[2], c="k", label='estimates')
#plt.axhline(y=2 * a2, c="r", label='true', ls="--")
#plt.legend()

plt.figure()
plt.xlabel("Time(Sec)")
plt.ylabel("error between xhat and true signal")
plt.plot(t, e0)
plt.plot(t, SP11, '--')
plt.plot(t, SP11D, '--')
plt.ylim(-1500, 1500)

plt.figure()
plt.xlabel("Time(Sec)")
plt.ylabel("error between d_xhat and true signal")
plt.plot(t, e1)
plt.plot(t, SP22, '--')
plt.plot(t, SP22D, '--')
plt.ylim(-400, 400)

#plt.figure()
#plt.xlabel("Time(Sec)")
#plt.ylabel("error between d2_xhat and true signal")
#plt.plot(t, e2)
#plt.plot(t, SP33, '--')
#plt.plot(t, SP33D, '--')
#plt.ylim(-200, 200)
plt.show()
